#include "Obstacles.h"


void Obstacles::processLaserData(const sensor_msgs::LaserScan::ConstPtr& scan) {
    obstacles.clear();
    int flag = 0;
    double noise_threshold = 0.5; // 设置噪声阈值，允许的最大距离差异

    for (size_t i = 0; i < scan->ranges.size() - 1; ++i) {
        double dis = scan->ranges[i];
        double angle = scan->angle_min + scan->angle_increment * i;

        if (dis > scan->range_min && dis < scan->range_max && dis <= 4.0) {
            // 检查是否为噪声
            /* if (i > 0 && abs(dis - scan->ranges[i - 1]) > noise_threshold && i < scan->ranges.size() - 1 && abs(dis - scan->ranges[i + 1]) < noise_threshold) {
                continue; // 跳过此点，认为是噪声
            } */
            // 调整角度，使其相对于正前方
            double adjusted_angle = angle + (M_PI / 2); // 假设正前方是0度，背后是180度
            double x = dis * cos(angle) + 0.08;
            double y = dis * sin(angle);
            if(flag%40 == 0 || abs(dis - scan->ranges[i + 1]) > 0.2)
            {
                obstacles.push_back(std::make_pair(x, y));
                if(abs(dis - scan->ranges[i + 1]) > 0.2)flag = 0;
            } 
            flag++;
        }
    }
}

void Obstacles::odomCallback(const nav_msgs::Odometry::ConstPtr& odom) {
    vehicle_x = odom->pose.pose.position.x;
    vehicle_y = odom->pose.pose.position.y;
    vehicle_yaw = tf::getYaw(odom->pose.pose.orientation);
}

void Obstacles::transformToMapCoordinates(double x, double y) {
    double cos_yaw = cos(vehicle_yaw);
    double sin_yaw = sin(vehicle_yaw);

    double map_x = vehicle_x + (x * cos_yaw - y * sin_yaw);
    double map_y = vehicle_y + (x * sin_yaw + y * cos_yaw);
    map_coordinates.push_back(std::make_pair(map_x, map_y));
    visualizeObstacles();

    /* std::cout << "Map Coordinates: ";
    for (const auto& coord : map_coordinates) {
        std::cout << "(" << coord.first << ", " << coord.second << ") ";
    }
    std::cout << std::endl; */
}

void Obstacles::visualizeObstacles() {
    visualization_msgs::Marker points;
    points.header.frame_id = "map";  // 设置参考系为地图
    points.header.stamp = ros::Time::now();
    points.ns = "Obstacles";
    points.action = visualization_msgs::Marker::ADD;
    points.pose.orientation.w = 1.0;

    points.id = 0;

    points.type = visualization_msgs::Marker::POINTS;

    // 设置点的大小
    points.scale.x = 0.2;
    points.scale.y = 0.2;

    // 设置颜色 (RGBA)
    points.color.r = 1.0f;
    points.color.g = 0.0f;
    points.color.b = 0.0f;
    points.color.a = 1.0;

    // 添加所有障碍物点到 marker
    for (const auto& coord : map_coordinates) {
        geometry_msgs::Point p;
        p.x = coord.first;
        p.y = coord.second;
        p.z = 0;
        points.points.push_back(p);
    }

    // 发布 marker
    marker_pub_.publish(points);
}

std::vector<myObstacle> Obstacles::getObstaclesAsPointers() {
    std::vector<myObstacle> obstacles_ptr;
    int obstacleCount = 0;
    for (const auto& coord : map_coordinates) {
    // 这里创建 myObstacle 对象，并使用 map_coordinates 的值。
    obstacles_ptr.push_back(myObstacle(coord.first, coord.second));
    obstacleCount++;
    }
    return obstacles_ptr;
}

// 激光数据回调函数
void Obstacles::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) {
    // 调用processLaserData函数处理激光数据
    map_coordinates.clear();
    processLaserData(scan);
    int obstacleCount = 0;
    for (const auto& obstacle : obstacles) {
        transformToMapCoordinates(obstacle.first, obstacle.second);
        obstacleCount++;
    }
}

Obstacles::Obstacles() {
    ros::NodeHandle nh;
    laser_ = nh.subscribe("/scan", 1000, &Obstacles::laserCallback, this);
    odom_ = nh.subscribe("/odom", 1000, &Obstacles::odomCallback, this);
    marker_pub_ = nh.advertise<visualization_msgs::Marker>("visualization_marker", 1);  // 初始化可视化 publisher
}

Obstacles::~Obstacles(){}
